// -*- coding: utf-8 -*-
#include "mcu_data_subscriber.h"
#include <ros/ros.h>
#include <ros/package.h>
#include <locale>
#include <codecvt>

McuDataSubscriber::McuDataSubscriber(ros::NodeHandle& nh) 
    : nh_(nh) {
    initializeDataTypes();
    ROS_INFO("McuDataSubscriber 初始化完成");
}

void McuDataSubscriber::initializeDataTypes() {
    // 尝试从配置文件加载数据类型映射
    std::string configFile = "/home/firefly/ws_pixhawk/src/pegasus/config/mcu_upstream_list.yaml";
    
    try {
        loadUpstreamConfig(configFile);
        ROS_INFO("McuDataSubscriber: 已从配置文件加载 %lu 个数据键类型映射", dataTypes_.size());
    } catch (const std::exception& e) {
        ROS_WARN("McuDataSubscriber: 加载配置文件失败: %s，使用默认配置", e.what());
        
        // 如果配置文件加载失败，使用默认的硬编码配置（与SerialPort保持一致）
        dataTypes_["button_basic"] = "double";
        dataTypes_["button_adv"] = "double";
        dataTypes_["button_cam_calib"] = "double";
        dataTypes_["sensor_voltage"] = "double";
        dataTypes_["sensor_current"] = "double";
        dataTypes_["sensor_temperature"] = "double";
        dataTypes_["device_status"] = "string";
        dataTypes_["error_message"] = "string";
        dataTypes_["firmware_version"] = "string";
        
        ROS_INFO("McuDataSubscriber: 已初始化 %lu 个默认数据键类型映射", dataTypes_.size());
    }
    
    // 为所有已知的数据键初始化默认值，确保getXXXValue函数总是能返回有效值
    initializeDefaultValues();
    
    // 立即为所有已知的数据键创建订阅器和发布器
    createAllSubscribers();
    createAllPublishers();
}

// === 同步数据获取接口 ===

std::string McuDataSubscriber::getStringValue(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    auto it = stringData_.find(key);
    return (it != stringData_.end()) ? it->second : "";
}

double McuDataSubscriber::getDoubleValue(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    auto it = doubleData_.find(key);
    return (it != doubleData_.end()) ? it->second : 0.0;
}

bool McuDataSubscriber::getButtonState(const std::string& buttonName) const {
    double value = getDoubleValue(buttonName);
    return (value == 1.0);
}

bool McuDataSubscriber::hasData(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    
    auto typeIt = dataTypes_.find(key);
    if (typeIt == dataTypes_.end()) {
        // 未知类型，检查两种数据容器
        return (stringData_.find(key) != stringData_.end()) || 
               (doubleData_.find(key) != doubleData_.end());
    }
    
    if (typeIt->second == "string") {
        return stringData_.find(key) != stringData_.end();
    } else {
        return doubleData_.find(key) != doubleData_.end();
    }
}

ros::Time McuDataSubscriber::getLastUpdateTime(const std::string& key) const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    auto it = updateTimes_.find(key);
    return (it != updateTimes_.end()) ? it->second : ros::Time(0);
}

// === 异步回调接口 ===

void McuDataSubscriber::subscribeString(const std::string& key, StringCallback callback) {
    stringCallbacks_[key] = callback;
    createSubscriber(key);
    ROS_INFO("已注册字符串数据回调: %s", key.c_str());
}

void McuDataSubscriber::subscribeDouble(const std::string& key, DoubleCallback callback) {
    doubleCallbacks_[key] = callback;
    createSubscriber(key);
    ROS_INFO("已注册数值数据回调: %s", key.c_str());
}

void McuDataSubscriber::subscribeButton(const std::string& buttonName, ButtonCallback callback) {
    // 按钮实际上是double类型数据
    doubleCallbacks_[buttonName] = [callback](double value) {
        callback(value == 1.0);
    };
    buttonCallbacks_[buttonName] = callback;
    createSubscriber(buttonName);
    ROS_INFO("已注册按钮状态回调: %s", buttonName.c_str());
}

void McuDataSubscriber::unsubscribe(const std::string& key) {
    auto subIt = subscribers_.find(key);
    if (subIt != subscribers_.end()) {
        subIt->second.shutdown();
        subscribers_.erase(subIt);
    }
    
    stringCallbacks_.erase(key);
    doubleCallbacks_.erase(key);
    buttonCallbacks_.erase(key);
    
    ROS_INFO("已取消订阅: %s", key.c_str());
}

// === 便捷接口 ===

bool McuDataSubscriber::waitForData(const std::string& key, double timeout_seconds) {
    ros::Time startTime = ros::Time::now();
    ros::Duration timeout(timeout_seconds);
    
    // 如果还没有订阅，先创建订阅器
    if (subscribers_.find(key) == subscribers_.end()) {
        createSubscriber(key);
        ros::Duration(0.1).sleep(); // 等待订阅器建立连接
    }
    
    ros::Rate rate(10); // 10Hz检查频率
    while (ros::ok() && (ros::Time::now() - startTime) < timeout) {
        if (hasData(key)) {
            return true;
        }
        ros::spinOnce();
        rate.sleep();
    }
    
    ROS_WARN("等待数据超时: %s (%.2f秒)", key.c_str(), timeout_seconds);
    return false;
}

std::vector<std::string> McuDataSubscriber::getSubscribedKeys() const {
    std::vector<std::string> keys;
    for (const auto& pair : subscribers_) {
        keys.push_back(pair.first);
    }
    return keys;
}

void McuDataSubscriber::printAllData() const {
    std::lock_guard<std::mutex> lock(dataMutex_);
    
    ROS_INFO("=== MCU数据状态 ===");
    
    ROS_INFO("字符串数据 (%lu项):", stringData_.size());
    for (const auto& pair : stringData_) {
        ros::Time updateTime = updateTimes_.at(pair.first);
        ROS_INFO("  %s = \"%s\" (更新时间: %.2f秒前)", 
                 pair.first.c_str(), 
                 pair.second.c_str(),
                 (ros::Time::now() - updateTime).toSec());
    }
    
    ROS_INFO("数值数据 (%lu项):", doubleData_.size());
    for (const auto& pair : doubleData_) {
        ros::Time updateTime = updateTimes_.at(pair.first);
        ROS_INFO("  %s = %.3f (更新时间: %.2f秒前)", 
                 pair.first.c_str(), 
                 pair.second,
                 (ros::Time::now() - updateTime).toSec());
    }
    
    ROS_INFO("已订阅话题 (%lu个):", subscribers_.size());
    for (const auto& pair : subscribers_) {
        ROS_INFO("  /serial/upstream/%s", pair.first.c_str());
    }
    
    ROS_INFO("==================");
}

// === 数据发布接口（模拟串口接收数据） ===

bool McuDataSubscriber::publishStringData(const std::string& key, const std::string& value) {
    auto pubIt = publishers_.find(key);
    if (pubIt == publishers_.end()) {
        ROS_ERROR("发布器不存在: %s (请检查配置文件中是否定义了此键)", key.c_str());
        return false;
    }
    
    try {
        std_msgs::String msg;
        msg.data = value;
        pubIt->second.publish(msg);
        
        // 更新本地存储的数据
        {
            std::lock_guard<std::mutex> lock(dataMutex_);
            stringData_[key] = value;
            updateTimes_[key] = ros::Time::now();
        }
        
        ROS_DEBUG("发布字符串数据: %s = %s", key.c_str(), value.c_str());
        return true;
    } catch (const std::exception& e) {
        ROS_ERROR("发布字符串数据失败: %s", e.what());
        return false;
    }
}

bool McuDataSubscriber::publishDoubleData(const std::string& key, double value) {
    auto pubIt = publishers_.find(key);
    if (pubIt == publishers_.end()) {
        ROS_ERROR("发布器不存在: %s (请检查配置文件中是否定义了此键)", key.c_str());
        return false;
    }
    
    try {
        std_msgs::Float64 msg;
        msg.data = value;
        pubIt->second.publish(msg);
        
        // 更新本地存储的数据
        {
            std::lock_guard<std::mutex> lock(dataMutex_);
            doubleData_[key] = value;
            updateTimes_[key] = ros::Time::now();
        }
        
        ROS_DEBUG("发布数值数据: %s = %.3f", key.c_str(), value);
        return true;
    } catch (const std::exception& e) {
        ROS_ERROR("发布数值数据失败: %s", e.what());
        return false;
    }
}

bool McuDataSubscriber::publishButtonState(const std::string& buttonName, bool pressed) {
    // 按钮状态用 double 类型发布，1.0 表示按下，0.0 表示释放
    double value = pressed ? 1.0 : 0.0;
    return publishDoubleData(buttonName, value);
}

bool McuDataSubscriber::hasUpstreamSubscribers(const std::string& key) const {
    auto pubIt = publishers_.find(key);
    if (pubIt != publishers_.end()) {
        return pubIt->second.getNumSubscribers() > 0;
    }
    return false;
}

uint32_t McuDataSubscriber::getUpstreamSubscriberCount(const std::string& key) const {
    auto pubIt = publishers_.find(key);
    if (pubIt != publishers_.end()) {
        return pubIt->second.getNumSubscribers();
    }
    return 0;
}

// === 私有方法 ===

void McuDataSubscriber::createSubscriber(const std::string& key) {
    // 避免重复创建
    if (subscribers_.find(key) != subscribers_.end()) {
        return;
    }
    
    std::string topicName = "/serial/upstream/" + key;
    
    // 根据数据类型创建相应的订阅器
    auto typeIt = dataTypes_.find(key);
    std::string dataType = (typeIt != dataTypes_.end()) ? typeIt->second : "double"; // 默认为double
    
    if (dataType == "string") {
        subscribers_[key] = nh_.subscribe<std_msgs::String>(
            topicName, 1, 
            [this, key](const std_msgs::String::ConstPtr& msg) {
                this->stringTopicCallback(key, msg);
            });
        ROS_INFO("订阅字符串话题: %s", topicName.c_str());
    } else {
        subscribers_[key] = nh_.subscribe<std_msgs::Float64>(
            topicName, 1,
            [this, key](const std_msgs::Float64::ConstPtr& msg) {
                this->doubleTopicCallback(key, msg);
            });
        ROS_INFO("订阅数值话题: %s", topicName.c_str());
    }
}

void McuDataSubscriber::stringTopicCallback(const std::string& key, const std_msgs::String::ConstPtr& msg) {
    {
        std::lock_guard<std::mutex> lock(dataMutex_);
        stringData_[key] = msg->data;
        updateTimes_[key] = ros::Time::now();
    }
    
    // 调用用户注册的回调函数
    auto callbackIt = stringCallbacks_.find(key);
    if (callbackIt != stringCallbacks_.end()) {
        callbackIt->second(msg->data);
    }
    
    ROS_DEBUG("收到字符串数据: %s = %s", key.c_str(), msg->data.c_str());
}

void McuDataSubscriber::doubleTopicCallback(const std::string& key, const std_msgs::Float64::ConstPtr& msg) {
    {
        std::lock_guard<std::mutex> lock(dataMutex_);
        doubleData_[key] = msg->data;
        updateTimes_[key] = ros::Time::now();
    }
    
    // 调用用户注册的数值回调函数
    auto doubleCallbackIt = doubleCallbacks_.find(key);
    if (doubleCallbackIt != doubleCallbacks_.end()) {
        doubleCallbackIt->second(msg->data);
    }
    
    // 调用用户注册的按钮回调函数（如果是按钮）
    auto buttonCallbackIt = buttonCallbacks_.find(key);
    if (buttonCallbackIt != buttonCallbacks_.end()) {
        buttonCallbackIt->second(msg->data == 1.0);
    }
    
    ROS_DEBUG("收到数值数据: %s = %.3f", key.c_str(), msg->data);
}

void McuDataSubscriber::loadUpstreamConfig(const std::string& configFile) {
    ROS_INFO("McuDataSubscriber: 正在加载MCU上行数据配置: %s", configFile.c_str());
    
    try {
        YAML::Node config = YAML::LoadFile(configFile);
        
        dataTypes_.clear(); // 清空现有配置
        
        int loadedCount = 0;
        for (const auto& item : config) {
            std::string keyName = item.first.as<std::string>();
            
            if (item.second.IsMap()) {
                YAML::Node keyConfig = item.second;
                
                // 获取数据类型
                std::string dataType = "double"; // 默认类型
                if (keyConfig["type"]) {
                    dataType = keyConfig["type"].as<std::string>();
                }
                
                // 验证数据类型
                if (dataType != "string" && dataType != "double") {
                    ROS_WARN("McuDataSubscriber: 配置键 '%s' 的数据类型 '%s' 无效，使用默认类型 'double'", 
                             keyName.c_str(), dataType.c_str());
                    dataType = "double";
                }
                
                // 存储配置
                dataTypes_[keyName] = dataType;
                loadedCount++;
                
                // 打印调试信息
                std::string description = keyConfig["description"] ? 
                    keyConfig["description"].as<std::string>() : "无描述";
                std::string topic = keyConfig["topic"] ? 
                    keyConfig["topic"].as<std::string>() : keyName;
                
                ROS_DEBUG("McuDataSubscriber: 加载数据键: %s (类型: %s, 话题: %s, 描述: %s)", 
                         keyName.c_str(), dataType.c_str(), topic.c_str(), description.c_str());
            } else {
                ROS_WARN("McuDataSubscriber: 跳过无效配置项: %s", keyName.c_str());
            }
        }
        
        ROS_INFO("McuDataSubscriber: 成功从配置文件加载了 %d 个数据键配置", loadedCount);
        
        if (loadedCount == 0) {
            throw std::runtime_error("配置文件中没有找到有效的数据键配置");
        }
        
    } catch (const YAML::Exception& e) {
        throw std::runtime_error("YAML解析错误: " + std::string(e.what()));
    } catch (const std::exception& e) {
        throw std::runtime_error("配置文件加载错误: " + std::string(e.what()));
    }
}

void McuDataSubscriber::initializeDefaultValues() {
    std::lock_guard<std::mutex> lock(dataMutex_);
    
    // 为所有已知的数据键预初始化默认值
    // 这样getXXXValue函数总是能返回有效值，而不是在首次访问时才创建
    for (const auto& keyTypePair : dataTypes_) {
        const std::string& key = keyTypePair.first;
        const std::string& type = keyTypePair.second;
        
        if (type == "string") {
            // 只有当键不存在时才初始化，避免覆盖已有数据
            if (stringData_.find(key) == stringData_.end()) {
                stringData_[key] = ""; // 字符串类型初始化为空字符串
                updateTimes_[key] = ros::Time(0); // 标记为未接收数据
                ROS_DEBUG("McuDataSubscriber: 预初始化字符串键 '%s'", key.c_str());
            }
        } else { // double类型
            // 只有当键不存在时才初始化，避免覆盖已有数据
            if (doubleData_.find(key) == doubleData_.end()) {
                doubleData_[key] = 0.0; // 数值类型初始化为0.0
                updateTimes_[key] = ros::Time(0); // 标记为未接收数据
                ROS_DEBUG("McuDataSubscriber: 预初始化数值键 '%s'", key.c_str());
            }
        }
    }
    
    ROS_INFO("McuDataSubscriber: 已预初始化 %lu 个数据键的默认值", dataTypes_.size());
}

void McuDataSubscriber::createAllSubscribers() {
    // 为所有已知的数据键立即创建订阅器
    for (const auto& keyTypePair : dataTypes_) {
        const std::string& key = keyTypePair.first;
        createSubscriber(key);
    }
    
    ROS_INFO("McuDataSubscriber: 已为 %lu 个数据键创建订阅器", dataTypes_.size());
}

void McuDataSubscriber::createAllPublishers() {
    // 为所有已知的数据键立即创建发布器
    for (const auto& keyTypePair : dataTypes_) {
        const std::string& key = keyTypePair.first;
        createPublisher(key);
    }
    
    ROS_INFO("McuDataSubscriber: 已为 %lu 个数据键创建发布器", dataTypes_.size());
}

void McuDataSubscriber::createPublisher(const std::string& key) {
    // 避免重复创建
    if (publishers_.find(key) != publishers_.end()) {
        return;
    }
    
    std::string topicName = "/serial/upstream/" + key;
    
    // 根据数据类型创建相应的发布器
    auto typeIt = dataTypes_.find(key);
    std::string dataType = (typeIt != dataTypes_.end()) ? typeIt->second : "double"; // 默认为double
    
    if (dataType == "string") {
        publishers_[key] = nh_.advertise<std_msgs::String>(topicName, 10);
        ROS_INFO("创建字符串发布器: %s", topicName.c_str());
    } else {
        publishers_[key] = nh_.advertise<std_msgs::Float64>(topicName, 10);
        ROS_INFO("创建数值发布器: %s", topicName.c_str());
    }
    
    // 等待连接建立
    ros::Duration(0.1).sleep();
}
